#ifndef SAY_SOMETHING_WITH_ROS_HPP_
#define SAY_SOMETHING_WITH_ROS_HPP_

#include <behaviortree_cpp/bt_factory.h>
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "test_interfaces/srv/test.hpp"

/**
 * @brief  StatefullActionNode 可以返回RUNNING状态
 */
class SaySomethingWithRos : public BT::StatefulActionNode
{
private:
    rclcpp::Node::SharedPtr node_;
    rclcpp::Client<test_interfaces::srv::Test>::SharedPtr client_;
    rclcpp::WaitSet wait_set_;
    // rclcpp::executors::MultiThreadedExecutor exector_;
    int sum;

public:
    SaySomethingWithRos(const std::string &name, const BT::NodeConfiguration &config) : BT::StatefulActionNode(name, config)
    {
        node_ = std::make_shared<rclcpp::Node>("test");
        client_ = node_->create_client<test_interfaces::srv::Test>("add_three_ints");
        wait_set_.add_client(this->client_);
        // exector_.add_node(node_);
    }

    ~SaySomethingWithRos()
    {
    }

    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface()
    {
        return node_->get_node_base_interface();
    }

    static BT::PortsList providedPorts()
    {
        return {BT::InputPort<std::string>("message")};
    }

    void callback(rclcpp::Client<test_interfaces::srv::Test>::SharedFuture future)
    {
        // 处理请求结果
        auto result = future.get();
        sum = result->sum;
        RCLCPP_INFO(node_->get_logger(), "got response");
        RCLCPP_INFO(node_->get_logger(), "result is %d", sum);
    }

    BT::NodeStatus onStart() override
    {
        auto request = std::make_shared<test_interfaces::srv::Test::Request>();
        request->a = 1;
        request->b = 2;
        request->c = 3;
        client_->async_send_request(request, std::bind(&SaySomethingWithRos::callback, this, std::placeholders::_1));

        return BT::NodeStatus::RUNNING;
    }

    bool isFinished()
    {
        auto wait_result = wait_set_.wait(std::chrono::seconds(0));
        if (wait_result.kind() == rclcpp::WaitResultKind::Ready)
        {
            if (wait_result.get_wait_set().get_rcl_wait_set().clients[0])
            {
                RCLCPP_INFO(node_->get_logger(), "Wait-set handle client");
                test_interfaces::srv::Test::Response response;
                rmw_request_id_t request_id;
                if (client_->take_response(response, request_id))
                {
                    sum = response.sum;
                }
                return true;
            }
        }
        else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout)
        {
            RCLCPP_ERROR(node_->get_logger(), "Wait-set failed with timeout");
            return false;
        }
        else
        {
            return false;
        }
    }

    BT::NodeStatus onRunning() override
    {
        if (!isFinished())
        {
            rclcpp::spin_some(node_);
            return BT::NodeStatus::RUNNING;
        }
        else
        {
            auto msg = getInput<std::string>("message");
            if (!msg)
            {
                throw BT::RuntimeError("missing parameter [message]");
            }

            std::cout << "say: " << msg.value() << "add result: " << sum << std::endl;
            return BT::NodeStatus::SUCCESS;
        }
    }

    void onHalted() override
    {

    }    
};

#endif